/* Serial.cc -- Serial
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogo.ramos@usp.br>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <iostream>

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <time.h>
#include <string.h>
#include <math.h>

#include "Serial.hh"

using namespace std;

#define ROBOT_RADIUS 0.09
#define MAX_VEL_MS 2
#define MIN_VEL_PWM 0

Serial::Serial()
{
}

Serial::~Serial()
{
}

int
Serial::Init( const string end,
	      const string serial_path )
{
    path_ = serial_path;

    if( end.size() != 3 ){
	cerr << "O endereço deve ter 3 letras" << endl;
	return 1;
    }
    

    fd_ = open(path_.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_ == -1)
    {
	cerr << "Não consegui abrir a serial" << endl;
	return 1;
    }

    speed_ = B9600;
    
    tcgetattr (fd_, &(options_));

    /* velocidade */
    cfsetspeed (&(options_), speed_);

    /* 8 bits por byte */
    options_.c_cflag |= CS8;

    tcsetattr (fd_, TCSANOW, &(options_) );

    command_size_ = 9;
    command_ = (unsigned char*) malloc( sizeof(unsigned char) * command_size_ );

    /* DEFINIÇÃO DA PALAVRA DO PROTOCOLO */
    /* BYTE 1 - primeiro byte do endereço */
    command_[0] = end[0];

    /* BYTE 2 - segundo byte do endereço */
    command_[1] = end[1];

    /* BYTE 3 - terceiro byte do endereço */
    command_[2] = end[2];

    /* BYTE 4 - byte de configuração, que engloba o chute, o rolinho e os sentidos dos motores */
    conf_ = &command_[3];
    command_[3] = 0x01;

    /* BYTE 5 - velocidade do motor 1 */
    motor_1_ = &command_[4];
    command_[4] = 0x00;

    /* BYTE 6 - velocidade do motor 2 */
    motor_2_ = &command_[5];
    command_[5] = 0x00;

    /* BYTE 7 - velocidade do motor 3 */
    motor_3_ = &command_[6];
    command_[6] = 0x00;

    /* BYTE 8 - checksum high */
    command_[7] = 0x00;

    /* BYTE 9 - checksum low */
    command_[8] = 0x00;
    /* FIM DA DEFINIÇÃO DA PALAVRA DO PROTOCOLO */

    delay_.tv_sec = 0;
    delay_.tv_nsec = 10000;

    command_repeat_ = 1;

    /* Criando matrizes de conversão */
    matrix_conv_ = new Matrix(3,3);
    matrix_conv_->set( 0, 0, 1.f );
    matrix_conv_->set( 0, 1, 0.5f );
    matrix_conv_->set( 0, 2, ROBOT_RADIUS );

    matrix_conv_->set( 1, 0, -1.f );
    matrix_conv_->set( 1, 1, 0.5f );
    matrix_conv_->set( 1, 2, ROBOT_RADIUS );

    matrix_conv_->set( 2, 0, 0.f );
    matrix_conv_->set( 2, 1, -1.f );
    matrix_conv_->set( 2, 2, ROBOT_RADIUS );

    /* Criando matriz que irá conter informações de velocidades
     * relativas vx, vy e va */
    matrix_vels_ = new Matrix( 3, 1 );

    /* Criando matriz que irá conter as velocidade de cada um dos 3
     * motores */
    matrix_motor_vels_ = new Matrix( 3, 1 );
	
}

void
Serial::robotKick( const int nivel )
{
    int repeat = 0;
    
    *(conf_) ^= 0x40;

    updateChecksun();

    repeat = command_repeat_;
    command_repeat_ = 1;
    sendCommand();
    command_repeat_ = repeat;
    
    *(conf_) &= 0xBF;
}

void
Serial::robotDribbleEnable( const int enable )
{
    int repeat = 0;

    if( enable )
	*(conf_) ^= 0x80;
    else
	*(conf_) &= 0x7F;

    updateChecksun();

    repeat = command_repeat_;
    command_repeat_ = 1;
    sendCommand();
    command_repeat_ = repeat;
}

void
Serial::robotSetVel( const float vx, const float vy, const float va )
{
    int v1 = 0;
    int v2 = 0;
    int v3 = 0;
    
    /* Atualiza matriz que será multiplicada */
    matrix_vels_->set( 0, 0, vx );
    matrix_vels_->set( 1, 0, vy );
    matrix_vels_->set( 2, 0, va );

    Matrix::mult( matrix_conv_,
		  matrix_vels_,
		  matrix_motor_vels_ );

    /* Atualiza velocidade dos motores */
    v1 = getMsAsPWM( matrix_motor_vels_->get( 0, 0 ) );
    v2 = getMsAsPWM( matrix_motor_vels_->get( 1, 0 ) );
    v3 = getMsAsPWM( matrix_motor_vels_->get( 2, 0 ) );

    matrix_motor_vels_->clean();

    /* Motor 1 */
    if( v1 < 0 )
    {
	*(motor_1_) = (unsigned char) v1 * -1;
	motorSetDirection( conf_, 1, 1 );
    }
    else
    {
	*(motor_1_) = (unsigned char) v1;
	motorSetDirection( conf_, 1, 0 );
    }

    /* Motor 2 */
    if( v2 < 0 )
    {
	*(motor_2_) = (unsigned char) v2 * -1;
	motorSetDirection( conf_, 2, 1 );
    }
    else
    {
	*(motor_2_) = (unsigned char) v2;
	motorSetDirection( conf_, 2, 0 );
    }

    /* Motor 3 */
    if( v3 < 0 )
    {
	*(motor_3_) = (unsigned char) v3 * -1;
	motorSetDirection( conf_, 3, 1 );
    }
    else
    {
	*(motor_3_) = (unsigned char) v3;
	motorSetDirection( conf_, 3, 0 );
    }

    updateChecksun();
    sendCommand();
}

void
Serial::updateChecksun()
{
    unsigned int total = 0;
    unsigned int roda = 0;
    int k = 0;
    
    /* Checksum */
    for (k = 0; k < command_size_ - 2; k++)
    {
	total += command_[k];
    }
 
    roda = total;
    roda >>= 8;
    command_[7] = (unsigned char) roda;
    command_[8] = (unsigned char) total;
}

void
Serial::sendCommand()
{
    int i = 0;

    for (i = 0; i < command_repeat_; i++)
    {
	if( write (fd_, command_, command_size_) < 0)
	    cerr << "Nao consegui escrever na serial" << endl;
	nanosleep (&(delay_), &(sobrando_) );
    }
}

// void
// Serial::setConf( const char conf )
// {
//     *(conf_) &= 0xF0;
//     *(conf_) ^= conf_;
// }

void
Serial::motorSetDirection( unsigned char * conf, int motor, int invert )
{
    switch( motor )
    {
    case 3:
    {
	if( invert )
	{
	    *(conf_) |= 0x01;
	}
	else
	{
	    *(conf_) &= 0xFE;
	}
	break;
    }
    case 2:
    {
	if( invert )
	{
	    *(conf_) |= 0x02;
	}
	else
	{

	    *(conf_) &= 0xFD;
	}
	break;
    }
    case 1:
    {
	if( invert )
	{
	    *(conf_) |= 0x04;
	}
	else
	{
	    *(conf_) &= 0xFB;
	}
	break;
    }
    }
}

int
Serial::getMsAsPWM( const float vel_ms )
{
    int sinal = 1;
    int pwm = 0;

    if( vel_ms < 0 )
	sinal = -1;

    pwm = sinal * (((255 * fabs(vel_ms))/MAX_VEL_MS) + MIN_VEL_PWM);

    return pwm;
}

void
Serial::robotSetVelPWM( const float vx, const float vy, const float va )
{
}
